#include "stdafx.h"

#include "PathingManager.h"
#include "Terrain/Terrain.h"
#include "graphics/Primitives.hpp"
#include "Obj/ObjContainer.h"

PathingManager* PathingManager::instance = nullptr;

namespace {

	float euclDist(const glm::vec2 & v0, const glm::vec2 & v1 ) {
		return glm::length(v0-v1);
	}

	bool nodeInBounds(int j, int i, int dimJ, int dimI) {
		return (i >= 0 && j >= 0 && i < dimI && j < dimJ);
	}
}
	
PathingManager* PathingManager::getInstance() {
	if (!instance) {
		(void)Terrain::getInstance(); // force init of dependency
		instance = new PathingManager();
	}
	return instance;
}

PathingManager::PathingManager() {
	reset();
}

std::vector<PathNode*> PathingManager::FindShortestPath(PathNode* startNode, PathNode* targetNode) {

	/*std::printf("Target at: %g %g\n", startNode->pos.x, startNode->pos.y);
	std::printf("Target at: %g %g\n", targetNode->pos.x, targetNode->pos.y);*/

	if (!startNode)
		throw std::runtime_error("Pathing: Start node is null\n");
	if (!targetNode)
		throw std::runtime_error("Pathing Target node is null\n");

	for (unsigned int i = 0; i < pathMap.size(); ++i) {
		pathMap[i]->f = euclDist(startNode->pos, targetNode->pos);
		pathMap[i]->g = 0.0f;
		pathMap[i]->backPtr = nullptr;
	}

	std::vector<PathNode*> closedSet;
	std::vector<PathNode*> openSet;
	openSet.push_back(startNode);

	startNode->g = 0.0f;
	int iii = 0;

	while (true){

		PathNode* bestNode = nullptr;
		float minF = 1e8f;
		// select best candidate from openset 
		for (unsigned int i = 0; i < openSet.size(); ++i) {
			if (openSet[i]->f < minF) {
				bestNode = openSet[i];
				minF = openSet[i]->f;
			}
		}

		if (!bestNode) {
			throw std::runtime_error("Best node in A* was NULL, openset is probably empty\n");
		}

		if (bestNode == targetNode)
			break;

		//std::printf("Sizof openset: %i\, evaluated %i\n", openSet.size(), iii++);
		/*std::printf("Closing node at: %g %g\n", bestNode->pos.x, bestNode->pos.y);*/

		openSet.erase(std::find(begin(openSet), end(openSet), bestNode));
		closedSet.push_back(bestNode);

		for ( int i = 0; i < PathNode::N_NEIGH; ++i) {
			PathNode* neighboor = bestNode->neigh[i];
			if (!neighboor) 
				continue;
			/*std::printf("neigh: %g %g\n", neighboor->pos.x, neighboor->pos.y);*/
			float candidateGScore = bestNode->g + euclDist(bestNode->pos, neighboor->pos);
			if ( ( std::find(begin(closedSet), end(closedSet), neighboor) != end(closedSet) ) && candidateGScore > neighboor->g) // if in closed set check if there is a  better path through this node
				continue;

			// if g score improvement or not yet in openset, open this node 
			if ( (candidateGScore < neighboor->g ) || ( std::find(begin(openSet), end(openSet), neighboor) == end(openSet) ) ) {
				// not previously seen or improved g-score neighboor, update:
				//std::printf("Improving or opening node at: %g %g\n", neighboor->pos.x, neighboor->pos.y);
				neighboor->backPtr = bestNode;
				neighboor->g = candidateGScore;
				neighboor->f = candidateGScore + euclDist(neighboor->pos, targetNode->pos);
				if (std::find(begin(openSet), end(openSet), neighboor) == end(openSet)) // if not already added, add 
					openSet.push_back(neighboor);
			}
		}
	}
	
	// reconstruct path 
	std::vector<PathNode*> path;

	PathNode* curNode = targetNode;
	while (true) {
		path.push_back(curNode);
		curNode = curNode->backPtr;
		if (curNode == startNode)
			break;
	}

	std::reverse(begin(path), end(path));

	return path;
}

void PathingManager::Move(std::vector<PlayerObj*> movers, const glm::vec2 & targetLoc) {
	
	glm::vec2 centreOfGravity(0.0f, 0.0f);
	for ( unsigned int i = 0; i < movers.size(); ++i) {
		centreOfGravity += movers[i]->getPos2();
	} centreOfGravity /= movers.size();

	glm::vec2 startPos = centreOfGravity;
	glm::ivec2 nodeIndex = glm::ivec2(startPos.x / nodeSize, startPos.y / nodeSize);
	PathNode* startNode = pathMap[nodeIndex.x + nodeIndex.y*nodeDim.x];
	if (!startNode->walkable)
		return;
	/*std::printf("StartNode: %i %i\tStartPos: %g %g\n", nodeIndex.x, nodeIndex.y, startPos.x, startPos.y);*/

	nodeIndex = glm::ivec2(targetLoc.x / nodeSize, targetLoc.y / nodeSize);
	PathNode* endNode = pathMap[nodeIndex.x + nodeIndex.y*nodeDim.x];
	if (!endNode->walkable)
		return;
	/*std::printf("EndNode: %i %i\tEndPos %g %g\n", nodeIndex.x, nodeIndex.y, targetLoc.x, targetLoc.y);*/
	
	if (nodeIndex.x < 0 || nodeIndex.y < 0 || nodeIndex.x >= nodeDim.x || nodeIndex.y >= nodeDim.y) {
		/*std::printf("ILLEGAL target node!\n");*/
		return;
	}
	if (startNode == endNode)
		return;

	/*std::printf("=======\n");*/

	pathingGroup pathingGroup(FindShortestPath(startNode, endNode), movers); 

	// find out if any of the movers used to belong to another pathing group, if so remove them from that pathing group 
	for		( unsigned int k = 0; k < movers.size(); ++k) {
	for		( unsigned int i = 0; i < pathingGroupList.size(); ++i) {
	for		( auto it = pathingGroupList[i].pathers.begin(); it != pathingGroupList[i].pathers.end(); ++it) {
				if ((*it)->mover == movers[k]) {
					pathingGroupList[i].pathers.erase(it);
					break;
				}
			}
		}
	}

	pathingGroupList.push_back(pathingGroup);
}

void PathingManager::update(float dt) {
	for (auto it = pathingGroupList.begin(); it != pathingGroupList.end(); ) {
		(*it).update(dt);
		if ((*it).finished) 
			it = pathingGroupList.erase(it);
		else 
			it++;
	}
}

// for debugging 
void PathingManager::draw(const glm::mat4 & viewMatrix)
{
	PointRenderer* pointRenderer = Game::getInstance()->pointRenderer;
	LineRenderer* lineRenderer = Game::getInstance()->lineRenderer;

	for ( unsigned int i = 0; i < pathingGroupList.size(); ++i) {
		// draw a line from each object to its current goal node 
		pathingGroup& group = pathingGroupList[i];
		if (group.attractorIndex < 0)
			continue;
		
		
		for (auto moverIt = pathingGroupList[i].pathers.begin(); moverIt != pathingGroupList[i].pathers.end(); ++moverIt) {
			glm::vec3 moverPos = (*moverIt)->mover->m_pos;
			glm::vec2 destPos = (*moverIt)->mover->getDest();
			
			lineRenderer->add(moverPos, glm::vec3(destPos.x, moverPos.y, destPos.y), glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));

			glm::vec3 moverF = glm::vec3((*moverIt)->mover->f.x, 0.0f, (*moverIt)->mover->f.y);
			lineRenderer->add(moverPos, moverPos+moverF, glm::vec4(1.0f, 0.0f, 0.0f, 1.0f));
		}
	}

	//glPointSize(5.0f);
#if 1 // render pathing graph  
	std::vector<PC3DVertex> pointVector(pathMap.size());
	for ( unsigned int i = 0; i < pathMap.size(); ++i) {
		float h = -9.5f; 
		/*float h = Terrain::getInstance()->sample(pathMap[i]->pos);*/
		if (pathMap[i]->walkable)
			pointVector[i] = PC3DVertex(glm::vec3(pathMap[i]->pos.x, h+0.05f, pathMap[i]->pos.y), glm::vec4(0.0f, 1.0f, 1.0f, 1.0));
		else 
			pointVector[i] = PC3DVertex(glm::vec3(pathMap[i]->pos.x, h+0.05f, pathMap[i]->pos.y), glm::vec4(1.0f, 0.0f, 0.0f, 1.0));
	} pointRenderer->add(pointVector);
#endif 

	/*Game::getInstance()->lineRenderer->add(glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(80.0f, 0.0, 50.0f), glm::vec4(1.0f, 0.0f, 0.0f, 1.0f));*/
	/*glPointSize(4.0f);*/
	pointRenderer->draw(viewMatrix);
	lineRenderer->draw(viewMatrix);
}

void PathingManager::reset() {

	Terrain* terrain = Terrain::getInstance();

	glm::vec4 terrainExtent = Terrain::getInstance()->getExtent();
	glm::vec2 startPos = glm::vec2(terrainExtent.x, terrainExtent.y);
	glm::vec2 terrainDim = glm::vec2(terrainExtent.z, terrainExtent.w);
	nodeSize = 4.0f;
	nodeDim = glm::ivec2(terrainDim.x / nodeSize, terrainDim.y / nodeSize);

	pathMap = std::vector<PathNode*>(nodeDim.x * nodeDim.y);
	for		(int i = 0; i < nodeDim.y; ++i) {
		for (int j = 0; j < nodeDim.x; ++j) {
			pathMap[j + i * nodeDim.x] = new PathNode();
			pathMap[j + i * nodeDim.x]->pos = glm::vec2(j*nodeSize+0.5f*nodeSize, i*nodeSize+0.5f*nodeSize);
			pathMap[j + i * nodeDim.x]->walkable = ObjContainer::getInstance()->walkable(pathMap[j + i * nodeDim.x]->pos, nodeSize);
		}
	}

	for		(int i = 0; i < nodeDim.y; ++i) {
		for (int j = 0; j < nodeDim.x; ++j) {
			
			PathNode* thisNode = pathMap[j + i * nodeDim.x];
			
			/*if ( (i > 5 && i < 7) && (j > 5 && j < 15))
				thisNode->walkable = false;

			if ( (i > 7 && i < 9) && (j > 3 && j < 13))
				thisNode->walkable = false;

			if ( (i > 9 && i < 11) && (j > 14 && j < 25))
				thisNode->walkable = false;*/

			if (!thisNode->walkable) 
				continue;

			int jj = j-1; int ii = i-1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable )
				pathMap[j + i * nodeDim.x]->neigh[0] = pathMap[jj + ii * nodeDim.x];

			jj = j; ii = i-1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable )				
				pathMap[j + i * nodeDim.x]->neigh[1] = pathMap[jj + ii * nodeDim.x];

			jj = j+1; ii = i-1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[2] = pathMap[jj + ii * nodeDim.x];

			jj = j+1; ii = i;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[3] = pathMap[jj + ii * nodeDim.x];

			jj = j+1; ii = i+1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[4] = pathMap[jj + ii * nodeDim.x];

			jj = j; ii = i+1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[5] = pathMap[jj + ii * nodeDim.x];

			jj = j-1; ii = i+1;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[6] = pathMap[jj + ii * nodeDim.x];

			jj = j-1; ii = i;
			if ( nodeInBounds(jj, ii, nodeDim.x, nodeDim.y ) && pathMap[jj + ii * nodeDim.x]->walkable)				
				pathMap[j + i * nodeDim.x]->neigh[7] = pathMap[jj + ii * nodeDim.x];
		}
	}
}

pathingGroup::pathingGroup(const std::vector<PathNode*> & path, const std::vector<PlayerObj*> & movers) :
	path(path), finished(false), attractorIndex(-1)
{
	pathers.resize(movers.size());
	for ( unsigned int i = 0; i < movers.size(); ++i)
		pathers[i] = new Pather(movers[i]);
}

void pathingGroup::update(float dt) {
	if (pathers.empty()) {
		finished = true;
		return;
	}
		
	glm::vec2 centreOfGravity(0.0f, 0.0f);
	for (unsigned int i = 0; i < pathers.size(); ++i ) {
		centreOfGravity += pathers[i]->mover->getPos2();
	}
	centreOfGravity /= pathers.size();
	
	/* properties to control movement behavior */
	const float separationDist = 5.0f;
	const float alignDist = 30.0f;
	const float cohesianDist = 15.0f;
	const float separatationStrength = 0.1f;
	const float alignStrength = 0.2f;
	const float cohesianStrength = 0.4f;

	for		(unsigned int i = 0; i < pathers.size(); ++i) {
		PlayerObj* obj0 = pathers[i]->mover;
		glm::vec2 sumV(0.0f, 0.0f);
		int alignN = 0;
		glm::vec2 sumPos(0.0f, 0.0f);
		int cohesianN = 0;
		for (unsigned int j = 0; j < pathers.size(); ++j) {
			if (i == j) continue;

			PlayerObj* obj1 = pathers[j]->mover;
			glm::vec3 dist = obj0->m_pos - obj1->m_pos;
			float distLen = glm::length(dist);

			if ( distLen < separationDist) {
				float forceStrength = separatationStrength * (separationDist - distLen) / separationDist;
				glm::vec3 nDist = glm::normalize(dist);
				glm::vec3 desiredV0 = nDist * obj0->maxV;
				/*glm::vec3 desiredV1 = -nDist* obj1->maxV;*/
				glm::vec2 f0 = glm::vec2(desiredV0.x, desiredV0.z) - obj0->v;
				/*glm::vec2 f1 = glm::vec2(desiredV1.x, desiredV1.z) - obj1->v;*/
				obj0->addForce(f0*forceStrength);
				/*obj1->addForce(f1*forceStrength);*/
			}

			if ( distLen < alignDist ) {
				alignN++;
				sumV += obj1->v;
			}

			if ( distLen < cohesianDist ) {
				cohesianN++;
				sumPos += glm::vec2(obj1->m_pos.x, obj1->m_pos.z);
			}
		}
		if (alignN > 0) {
			sumV /= alignN;
			glm::vec2 f = sumV;// - obj0->v;
			obj0->addForce(f*alignStrength);
		}
		if (cohesianN > 0) {
			sumPos /= cohesianN;
			glm::vec2 cohDesiredV = sumPos - glm::vec2(obj0->m_pos.x, obj0->m_pos.z);
			glm::vec2 f = cohDesiredV;// - obj0->v;
			obj0->addForce(f*cohesianStrength);
		}
	}

	/* Updade waypoints for this pathingroup */

	if (attractorIndex == -1) {
		attractorIndex = 0;
		for ( unsigned int i = 0; i < pathers.size(); ++i) {
			pathers[i]->mover->setDest(path[attractorIndex]->pos);
		}
	}

#if 0 // path centre of gravity through waypoints, maybe say local centre of gravity -> new waypoint 
	if ( glm::length(path[attractorIndex]->pos - centreOfGravity) < (1.0f + 1.5*pathers.size()) ) {
		if ( ++attractorIndex < (int)path.size()) {
			for ( unsigned int i = 0; i < pathers.size(); ++i) {
				pathers[i]->mover->setDest(path[attractorIndex]->pos);
			}
		}
		else {
			for ( unsigned int i = 0; i < pathers.size(); ++i) {
				pathers[i]->mover->stopPathing();
			}
			finished = true;
		}
	}

#else // individually path everyone through all waypoints 
	
	for ( unsigned int i = 0; i < pathers.size(); ++i ) {
		Pather* pather = pathers[i];
		if (pather->finished) continue;

		if (pather->pathIndex == -1) {
			pather->pathIndex = 0;
			pather->mover->setDest(path[pather->pathIndex]->pos);
		}

		if ( ( glm::length(path[pather->pathIndex]->pos - pather->mover->getPos2()) ) < (1.0f + 0.1*pathers.size()) ) {
			if ( ++(pather->pathIndex) < (int)path.size()) {
				pather->mover->setDest(path[pather->pathIndex]->pos);
			}
			else {
				pather->finished = true;
				pather->mover->stopPathing();
			}
		}
	}

	finished = true;
	for ( unsigned int i = 0; i < pathers.size(); ++i) {
		if (!pathers[i]->finished) {
			finished = false;
			break;
		}
	}
#endif 
}